
#define __MAIN_CPP__

#include "odrive_main.h"
#include "freertos_vars.h"
#include "fmc.h"


#include <communication/interface_can.hpp>
ODriveCAN::Config_t can_config;
Encoder::Config_t encoder_configs[AXIS_COUNT];
Controller::Config_t controller_configs[AXIS_COUNT];
Motor::Config_t motor_configs[AXIS_COUNT];
Axis::Config_t axis_configs[AXIS_COUNT];

std::array<Axis*, AXIS_COUNT> axes;
ODriveCAN *odCAN = nullptr;
ODrive odrv{};

extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
    for(auto& axis : axes){
        safety_critical_disarm_motor_pwm(axis->motor_);
    }
        safety_critical_disarm_brake_resistor(); 
    for (;;); // TODO: safe action
}
void vApplicationIdleHook(void) {
    if (odrv.system_stats_.fully_booted) {
        odrv.system_stats_.uptime = xTaskGetTickCount();
        odrv.system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
        //odrv.system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
        odrv.system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
        odrv.system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
        //odrv.system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
        //odrv.system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
        odrv.system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
        odrv.system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
        //odrv.system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);

        // Actual usage, in bytes, so we don't have to math
        odrv.system_stats_.stack_usage_axis0 = axes[0]->stack_size_ - odrv.system_stats_.min_stack_space_axis0;
        odrv.system_stats_.stack_usage_axis1 = axes[1]->stack_size_ - odrv.system_stats_.min_stack_space_axis1;
        //odrv.system_stats_.stack_usage_comms = stack_size_comm_thread - odrv.system_stats_.min_stack_space_comms;
        //odrv.system_stats_.stack_usage_usb = stack_size_usb_thread - odrv.system_stats_.min_stack_space_usb;
        //odrv.system_stats_.stack_usage_uart = stack_size_uart_thread - odrv.system_stats_.min_stack_space_uart;
        odrv.system_stats_.stack_usage_usb_irq = stack_size_usb_irq_thread - odrv.system_stats_.min_stack_space_usb_irq;
        odrv.system_stats_.stack_usage_startup = stack_size_default_task - odrv.system_stats_.min_stack_space_startup;
        //odrv.system_stats_.stack_usage_can = odCAN->stack_size_ - odrv.system_stats_.min_stack_space_can;
    }
}
}
void ODrive::save_configuration(void) {
    config_save(5,
    &can_config,sizeof(can_config),
    encoder_configs,sizeof(encoder_configs),
    controller_configs,sizeof(controller_configs),
    motor_configs,sizeof(motor_configs),
    axis_configs,sizeof(axis_configs));
}

extern "C" int load_configuration(void) {
    delay_1ms(1);
    config_load(5,
    &can_config,sizeof(can_config),
    encoder_configs,sizeof(encoder_configs),
    controller_configs,sizeof(controller_configs),
    motor_configs,sizeof(motor_configs),
    axis_configs,sizeof(axis_configs));
}
extern "C" int construct_objects(){
    odCAN = new ODriveCAN(can_config,CAN0);
    axis_configs[0].load_set_can_node_id(1);
    axis_configs[0].load_set_can_times(1);
    axis_configs[1].load_set_can_node_id(3);
    axis_configs[1].load_set_can_times(0);
    for (size_t i = 0; i < AXIS_COUNT; ++i) {
        Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
                                       encoder_configs[i]);
        Controller *controller = new Controller(controller_configs[i]);
        Motor *motor = new Motor(hw_configs[i].motor_config,
                                 hw_configs[i].gate_driver_config,
                                 motor_configs[i]);
        axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
                *encoder,*controller, 
                *motor);

        controller_configs[i].parent = controller;
        encoder_configs[i].parent = encoder;
        motor_configs[i].parent = motor;
        axis_configs[i].parent = axes[i];
    }
    return 0;
}
int odrive_main(void) {
    // Start ADC for temperature measurements and user measurements
    start_general_purpose_adc();
    osDelay(100);
    // Init communications (this requires the axis objects to be constructed)
    init_communication();
    // Start pwm-in compare modules
    // must happen after communication is initialized

	//pwm_in_enc();

    // Setup motors (DRV8301 SPI transactions here)
    for(auto& axis : axes){
        axis->motor_.setup();
    }
 
    // Setup encoders (Starts encoder SPI transactions)
    for(auto& axis : axes){
        axis->encoder_.setup();
    }

    // Setup anything remaining in each axisin
    for(auto& axis : axes){
        axis->setup();
    }

    // Start PWM and enable adc interrupts/callbacks
    start_adc_pwm();

    // This delay serves two purposes:
    //  - Let the current sense calibration converge (the current
    //    sense interrupts are firing in background by now)
    //  - Allow a user to interrupt the code, e.g. by flashing a new code,
    //    before it does anything crazy
    // TODO make timing a function of calibration filter tau
    osDelay(500);

    // Start state machine threads. Each thread will go through various calibration
    // procedures and then run the actual controller loops.
    // TODO: generalize for AXIS_COUNT != 2


    for (size_t i = 0; i < AXIS_COUNT; ++i) {	
        axes[i]->start_thread();
    }
	
    osDelay(100);
    axes[0]-> clear_errors();
	axes[1]-> clear_errors();
    printf("start\r\n");
    odrv.system_stats_.fully_booted = true;
	
    return 0;
}
